import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

#读取数据
df=pd.read_csv('KalmanFilter.csv')
        #获取true_position
true_position=np.float64(np.array(df.iloc[2:33,1]))
        #获取true_velocity
true_velocity=np.float64(np.array(df.iloc[2:33,2]))
        #获取位置过程误差
position_process_error=np.float64(np.array(df.iloc[2:33,3]))
        #获取速度过程误差
velocity_process_error=np.float64(np.array(df.iloc[2:33,4]))

#获取测量位置
measurement_position=np.float64(np.array(df.iloc[3:33,7]))
#获取测量速度
measurement_velocity=np.float64(np.array(df.iloc[3:33,8]))
#组成观测矩阵
measurement_matrix=np.array([measurement_position,measurement_velocity]).T
#测量位置误差
measurement_position_error=np.float64(np.array(df.iloc[3:33,9]))
#测量速度误差
measurement_velocity_error=np.float64(np.array(df.iloc[3:33,10]))
#iloc函数是左开右闭区间
class KalmanFilter:
    def __init__(self):
        self.A=np.array([[1,1],[0,1]])
        self.H=np.array([[1,0],[0,1]])
        self.Q=np.array([[0.1,0],[0,0.1]])
        self.R=np.array([[1,0],[0,1]])
        #初始状态估计
        self.x_0=np.array([0,0])
        #初始状态估计误差协方差矩阵
        self.P_0=np.array([[1,0],[0,1]])
        #先验状态估计
        self.x_hat=[]
        #先验状态估计误差协方差矩阵
        self.P_hat=[]
        #卡尔曼增益
        self.K=[]
        #后验状态估计
        self.x_k=[]
        #后验状态估计误差协方差矩阵
        self.P_k=[]

    #计算先验状态估计
    def prior_state_estimate(self,x_k):
        x_k_hat=self.A@x_k
        return x_k_hat

    #计算先验协方差估计
    def prior_covariance_estimate(self,P_k):
        P_k_hat=self.A@P_k@self.A.T+self.Q
        return P_k_hat

    #计算卡尔曼增益
    def kalman_gain(self,P_k_hat):
        K=(P_k_hat@self.H.T)@np.linalg.inv(self.H@P_k_hat@self.H.T+self.R)
        return K

    #计算后验状态估计
    def posterior_state_estimate(self,x_k_hat,K,z_k):
        x_k=x_k_hat+K@(z_k-self.H@x_k_hat)
        return x_k

    #计算后验协方差估计
    def posterior_covariance_estimate(self,P_k_hat,K):
        P_k=(np.eye(2)-K@self.H)@P_k_hat
        return P_k
    #卡尔曼滤波
    def kalman_filter(self):
        temp_x=self.x_0
        temp_P=self.P_0
        for i in range(measurement_matrix.shape[0]):
            self.x_hat.append(self.prior_state_estimate(temp_x))
            temp_x_hat=self.x_hat[i]
            self.P_hat.append(self.prior_covariance_estimate(temp_P))
            temp_P_hat=self.P_hat[i]
            self.K.append(self.kalman_gain(temp_P_hat))
            temp_K=self.K[i]
            self.x_k.append(self.posterior_state_estimate(temp_x_hat,temp_K,measurement_matrix[i]))
            self.P_k.append(self.posterior_covariance_estimate(temp_P_hat,temp_K))
            temp_x=self.x_k[i]
            temp_P=self.P_k[i]
if __name__ == '__main__':
    kalman_filter=KalmanFilter()
    kalman_filter.kalman_filter()
    plt.plot(np.array(kalman_filter.x_k).T[0],label='position')
    plt.plot(np.array(kalman_filter.x_hat).T[0],label='position_hat')
    plt.plot(measurement_position,label='measurement_position')
    plt.plot(true_position,label='true_position')
    plt.legend()
    plt.show()
    plt.plot(np.array(kalman_filter.x_k).T[1],label='velocity')
    plt.plot(np.array(kalman_filter.x_hat).T[1],label='velocity_hat')
    plt.plot(measurement_velocity,label='measurement_velocity')
    plt.plot(true_velocity,label='true_velocity')
    plt.legend()
    plt.show()















